/**
 * @file countdown_action_client.cpp
 * @brief ROS2 Action Client 示例：连接到倒计时 Action Server
 * 
 * 本文件演示了如何创建一个 ROS2 Action Client：
 * 1. 连接到 Action Server
 * 2. 发送倒计时目标（Goal）
 * 3. 接收执行过程中的反馈（Feedback）
 * 4. 接收最终结果（Result）
 */

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <chrono>
#include <future>
#include <memory>

#include "simple_action_demo/action/countdown.hpp"

using namespace std::chrono_literals;

/**
 * @class CountdownActionClient
 * @brief 倒计时 Action Client 类
 * 
 * 继承自 rclcpp::Node，实现倒计时 Action 客户端功能
 */
class CountdownActionClient: public rclcpp::Node
{
public:
    // 类型别名，简化代码
    using Countdown = simple_action_demo::action::Countdown;
    using GoalHandleCountdown = rclcpp_action::ClientGoalHandle<Countdown>;
    
    /**
     * @brief 构造函数
     * @param options ROS2 节点选项
     * 
     * 初始化 Action Client，创建定时器等待服务器就绪后发送目标
     */
    explicit CountdownActionClient(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
    :Node("countdown_action_client", options)
    {
        // 创建 Action Client
        // 参数说明：
        // - this: 当前节点指针
        // - "countdown": Action 名称（必须与 Server 的名称一致）
        client_ptr_ = rclcpp_action::create_client<Countdown>(this, "countdown");
        
        // 创建定时器，1秒后尝试发送目标
        // 这样可以给 Server 一些时间启动
        timer_ = this->create_wall_timer(
            1s,
            std::bind(&CountdownActionClient::send_goal, this));
    }
    /**
     * @brief 发送倒计时目标
     * 
     * 此函数会：
     * 1. 等待 Action Server 就绪
     * 2. 创建目标消息
     * 3. 配置回调函数
     * 4. 异步发送目标
     */
    void send_goal()
    {
        // 取消定时器（只发送一次目标）
        timer_->cancel();
        
        // 等待 Action Server 可用（最多等待10秒）
        // 如果 Server 未启动，这里会超时并返回 false
        if (!client_ptr_->wait_for_action_server(10s)) {
            RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
            rclcpp::shutdown();
            return;
        }
        RCLCPP_INFO(this->get_logger(), "Action server available, sending goal");
        
        // 创建目标消息
        auto goal_msg = Countdown::Goal();
        goal_msg.start_number = 5;  // 默认从5开始倒计时
        
        // 配置发送目标的选项，包括三个回调函数：
        auto send_goal_options = rclcpp_action::Client<Countdown>::SendGoalOptions();
        
        // 目标响应回调：当 Server 接受或拒绝目标时调用
        send_goal_options.goal_response_callback =
            std::bind(&CountdownActionClient::goal_response_callback, this, std::placeholders::_1);
        
        // 反馈回调：当 Server 发布反馈时调用（倒计时过程中）
        send_goal_options.feedback_callback =
            std::bind(&CountdownActionClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
        
        // 结果回调：当任务完成或失败时调用
        send_goal_options.result_callback =
            std::bind(&CountdownActionClient::result_callback, this, std::placeholders::_1);
        
        // 异步发送目标（非阻塞）
        // Server 会在 handle_goal 回调中处理此目标
        RCLCPP_INFO(this->get_logger(), "Sending countdown goal from %d", goal_msg.start_number);
        client_ptr_->async_send_goal(goal_msg, send_goal_options);
    }
    /**
     * @brief 取消目标（示例函数，未完全实现）
     * 
     * 要完整实现取消功能，需要保存 goal_handle，
     * 然后调用 client_ptr_->async_cancel_goal(goal_handle)
     */
    void cancel_goal()
    {
        RCLCPP_INFO(this->get_logger(),"Cancel feature demo - implement by storing goal_handle");
    }
    
private:
    rclcpp_action::Client<Countdown>::SharedPtr client_ptr_;  // Action Client 实例
    rclcpp::TimerBase::SharedPtr timer_;                       // 定时器，用于延迟发送目标
    rclcpp::TimerBase::SharedPtr cancel_timer_;                // 取消定时器（未使用）

    /**
     * @brief 目标响应回调函数
     * @param goal_handle 目标句柄（如果为空表示目标被拒绝）
     * 
     * 当 Server 处理目标请求后（接受或拒绝），此函数会被调用。
     * 如果 goal_handle 为空，表示目标被拒绝。
     */
    void goal_response_callback(const GoalHandleCountdown::SharedPtr & goal_handle)
    {
        if (!goal_handle) {
            // 目标被拒绝
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
        } else {
            // 目标被接受，等待执行结果
            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
        }
    }

    /**
     * @brief 反馈回调函数
     * @param goal_handle 目标句柄（未使用）
     * @param feedback 反馈消息（包含当前倒计时数字）
     * 
     * 在 Server 执行任务过程中，会定期调用此函数。
     * 可以在这里实时显示任务进度。
     */
    void feedback_callback(
        GoalHandleCountdown::SharedPtr,
        const std::shared_ptr<const Countdown::Feedback> feedback)
    {
        RCLCPP_INFO(this->get_logger(), "Received feedback: Current number = %d", feedback->current_number);
    }

    /**
     * @brief 结果回调函数
     * @param result 包装的结果对象，包含结果代码和结果数据
     * 
     * 当任务完成（成功、失败或取消）时，此函数会被调用。
     * 这是接收最终结果的地方。
     */
    void result_callback(const GoalHandleCountdown::WrappedResult & result)
    {
        // 根据结果代码处理不同的情况
        switch (result.code) {
            case rclcpp_action::ResultCode::SUCCEEDED:
                // 任务成功完成
                RCLCPP_INFO(this->get_logger(), "Goal succeeded!");
                break;
            case rclcpp_action::ResultCode::ABORTED:
                // 任务被中止（Server 内部错误）
                RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
                return;
            case rclcpp_action::ResultCode::CANCELED:
                // 任务被取消
                RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
                return;
            default:
                // 未知的结果代码
                RCLCPP_ERROR(this->get_logger(), "Unknown result code");
                return;
        }

        // 显示最终结果：完整的倒计时序列
        RCLCPP_INFO(this->get_logger(), "Final countdown sequence:");
        for (size_t i = 0; i < result.result->countdown_sequence.size(); ++i) {
            RCLCPP_INFO(this->get_logger(), "  Step %zu: %d", i, result.result->countdown_sequence[i]);
        }
        
        // 任务完成，关闭 ROS2
        rclcpp::shutdown();
    }
};

/**
 * @brief 主函数
 * @param argc 命令行参数数量
 * @param argv 命令行参数数组
 * @return 程序退出码
 * 
 * 创建并运行 Action Client 节点
 */
int main(int argc, char** argv)
{
    // 初始化 ROS2
    rclcpp::init(argc, argv);
    
    // 创建 Action Client 节点
    auto node = std::make_shared<CountdownActionClient>();
    
    // 进入事件循环，处理回调函数
    rclcpp::spin(node);
    
    // 关闭 ROS2
    rclcpp::shutdown();
    return 0;
}